#pragma once
#include <opencv2/opencv.hpp>
#include <iostream>
#include <chrono>
#include <fstream>

using namespace cv;
using namespace std;

extern float PI;

struct Edge
{
    Point up_point;
    Point down_point;
    Point center;
};

/**
 * @brief 姿态解算
 * 
 */
class SolvePnP
{
private:
    /* data */
    String video_path;
    Mat camera_matrix;           // 相机内参
    Mat distortion_coefficients; // 畸变参数
    float armor_width;
    float armor_length;
    int delay; // 每帧延迟时间

    Mat src;
    vector<Mat> channels;
    Mat r_channel;
    Mat threshold_mask;
    Mat contours_mask;
    vector<vector<Point>> contours;
    vector<Vec4i> hierarchy;

    vector<Point3f> obj_points;
    vector<Point2f> img_points;
    vector<float> rvec;
    vector<float> tvec;

    Mat r_mat;
    Mat t_mat;

    float theta_x;
    float theta_y;
    float theta_z;

public:
    SolvePnP(const String &path);
    ~SolvePnP() = default;
    void run();

    // 任务一中函数
    inline float getDistance4points(const Point &a, const Point &b) { return sqrtf((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y)); }
    inline float getNorm(const vector<float> &vector)
    {
        float sum = 0.f;
        for (auto &portion : vector)
        {
            sum += portion * portion;
        }
        return sqrtf(sum);
    }
    bool judgeArmor(const Edge &a, const Edge &b, const float upper_limit, const float lower_limit);
    inline Point2f getPointbyAngleAndDistance(const Point2f center, const float angle, const float distance) { return Point2f(center.x + sinf(angle * PI / 180) * distance, center.y - cosf(angle * PI / 180) * distance); };
};
